#include "SceneNode.h"

namespace Nezha
{

	SceneNode::SceneNode(const _string& name)
		:mName(name)
		,mLod(0)
		,mSceneLayer(RL_DEFAULT)
		,mCullingMask(NEZHA_CULLING_MASK_COMMON)
		,mParentSceneNode(NULL)
		,mPosition(Vector3::ZERO)
		,mWorldPos(Vector3::ZERO)
		,mScale(1, 1, 1)
		,mWorldScale(1, 1, 1)
		,mOrientation(Quaternion::IDENTITY)
		,mWorldOrientation(Quaternion::IDENTITY)
		,mHMatrixL2W(Matrix4x4::IDENTITY)
		,mControllerJoint(NULL)
		,mCustomCulling(NULL)
	{
	}

	SceneNode::~SceneNode()
	{
		// TODO

		if(mControllerJoint)
		{
			NZ_Delete mControllerJoint;
		}
	}

	void SceneNode::setPosition(const Vector3& pos)
	{
		mPosition = pos;
		NZSetBitTrue(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM | SOU_WORLDTRANSFORM_MATRIX | SOU_CHILDREN);
	}

	void SceneNode::setScale(const Vector3& scale)
	{
		NZ_ASSERT(scale.x != 0 && scale.y != 0 && scale.z != 0);

		mScale = scale;
		NZSetBitTrue(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM | SOU_WORLDTRANSFORM_MATRIX | SOU_CHILDREN);
	}

	void SceneNode::setOrientation(const Quaternion& q)
	{
		mOrientation = q;
		mOrientation.normalize();
		NZSetBitTrue(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM | SOU_WORLDTRANSFORM_MATRIX | SOU_CHILDREN);
	}

	void SceneNode::translate(const Vector3& trans, bool localSpace /* = true */)
	{
		if(localSpace)
		{
			mPosition += mOrientation * trans;
		}
		else
		{
			if(getParentSceneNode())
			{
				mPosition += (getParentSceneNode()->getWorldOrientation().inverse() * trans) / mScale;
			}
			else
			{
				mPosition += trans;
			}
		}

		NZSetBitTrue(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM | SOU_WORLDTRANSFORM_MATRIX | SOU_CHILDREN);
	}

	void SceneNode::rotate(const Matrix3x3& rotMat, bool localSpace /* = true */)
	{
		Quaternion q;
		q.fromRotation(rotMat);
		rotate(q, localSpace);
	}

	void SceneNode::rotate(const Quaternion& q, bool localSpace /* = true */)
	{
		Quaternion p = q;
		p.normalize();

		if(localSpace)
		{
			mOrientation = mOrientation * p;
		}
		else
		{
			/**
			remarks concatenate sequence !!
				q * world = parent * L <=> world = parent * L0
				q * world = (world * L0^-1) * L
				L = (L0 * world^-1) * q * world
			*/
			mOrientation = mOrientation * getWorldOrientation().inverse() * p * getWorldOrientation();
		}

		NZSetBitTrue(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM | SOU_WORLDTRANSFORM_MATRIX | SOU_CHILDREN);
	}

	const Matrix4x4& SceneNode::getWorldTransform() const
	{
		updateTransformFromParent();

		if(NZTestFlag(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM_MATRIX))
		{
			Matrix3x3 rotMat;
			mWorldOrientation.toRotation(rotMat);
			rotMat.timesDiagonal(mWorldScale);
			mHMatrixL2W.setRow(0, rotMat[0][0], rotMat[0][1], rotMat[0][2], mWorldPos.x);
			mHMatrixL2W.setRow(1, rotMat[1][0], rotMat[1][1], rotMat[1][2], mWorldPos.y);
			mHMatrixL2W.setRow(2, rotMat[2][0], rotMat[2][1], rotMat[2][2], mWorldPos.z);
			mHMatrixL2W.setRow(3, 0, 0, 0, 1.0f);

			NZSetBitFalse(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM_MATRIX);
		}

		return mHMatrixL2W;
	}

	void SceneNode::updateTransformFromParent() const
	{
		if(!NZTestFlag(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM))
		{
			if(!mParentSceneNode || !mParentSceneNode->getTransformDirty())
			{
				return;
			}
		}

		// update from parent
		if(mParentSceneNode)
		{
			const Quaternion& parentQuat = mParentSceneNode->getWorldOrientation();
			const Vector3& parentScale = mParentSceneNode->getWorldScale();

			mWorldOrientation = parentQuat * mOrientation;
			mWorldScale = parentScale * mScale;

			mWorldPos = parentQuat * (parentScale * mPosition);
			mWorldPos += mParentSceneNode->getWorldPos();
		}
		else
		{
			mWorldPos = mPosition;
			mWorldScale = mScale;
			mWorldOrientation = mOrientation;
		}

		NZSetBitTrue(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM_MATRIX);
	}

	const Vector3& SceneNode::getWorldPos() const
	{
		updateTransformFromParent();	

		return mWorldPos;
	}

	const Vector3& SceneNode::getWorldScale() const
	{
		updateTransformFromParent();

		return mWorldScale;
	}

	const Quaternion& SceneNode::getWorldOrientation() const
	{
		updateTransformFromParent();

		return mWorldOrientation;
	}

	void SceneNode::update(float dt)
	{
		// TODO

		if(mControllerJoint)
		{
			mControllerJoint->update(dt);
		}
	}

	void SceneNode::updateTransformNBounds(float dt, u32 flag /* = TransformUpdateBits */)
	{
		mTransformNBoundUpdateFlag |= flag;

		if(NZTestFlag(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM))
		{
			if(getParentSceneNode())
			{
				SceneNode* parentNode = getParentSceneNode();
				const Quaternion& p = parentNode->getWorldOrientation();
				mWorldOrientation = p * mOrientation;

				const Vector3& ps = parentNode->getWorldScale();
				mWorldScale = mScale * ps;

				mWorldPos = p * (ps * mPosition) + parentNode->getWorldPos();
			}
			else
			{
				mWorldOrientation = mOrientation;
				mWorldPos = mPosition;
				mWorldScale = mScale;
			}

			NZSetBitFalse(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM);
			NZSetBitTrue(mTransformNBoundUpdateFlag, SOU_WORLDTRANSFORM_MATRIX);
		}

		if(NZTestFlag(mTransformNBoundUpdateFlag, SOU_CHILDREN))
		{
			for(PArray<SceneNode*>::iterator it = mChildren.begin(); it != mChildren.end(); it++)
			{
				if((*it))
				{
					(*it)->updateTransformNBounds(dt, SOU_WORLDTRANSFORM | SOU_CHILDREN);
				}
			}
		}

		getWorldAABB(mTransformNBoundUpdateFlag);
	}

	void SceneNode::updateGeometryBound(AABB& aabb) const
	{
	}

	void SceneNode::upwardSetBoundsDirty() const
	{
		if(mParentSceneNode)
		{
			// make bounds-dirty signal upwards.
			SceneNode* parent = mParentSceneNode;

			while(parent)
			{
				NZSetBitTrue(parent->mTransformNBoundUpdateFlag, SOU_BOUND_UPWARD);
				parent = parent->mParentSceneNode;
			}
		}
	}

	const AABB& SceneNode::getWorldAABB(u32 updateFlag) const
	{
		if(updateFlag == 0)
		{
			return mWorldAABB;
		}

		// if children changed.
		if(NZTestFlag(mTransformNBoundUpdateFlag, SOU_BOUND_UPWARD))
		{
			NZSetBitTrue(updateFlag, SOU_BOUND_DOWNWARD);
		}

		if(NZTestFlag(mTransformNBoundUpdateFlag, SOU_BOUND_GEOMETRY))
		{
			NZSetBitTrue(updateFlag, SOU_BOUND_GEOMETRY);
		}

		if(updateFlag == SOU_BOUND_GEOMETRY)
		{
			updateGeometryBound(mWorldAABB);
			const Matrix4x4& HTransform = getWorldTransform();
			mWorldAABB.rebuildAffine(HTransform);
		}

		if(NZTestFlag(updateFlag, SOU_BOUND_DOWNWARD))
		{
			for(SceneNodeList::const_iterator it = mChildren.begin(); it != mChildren.end(); it++)
			{
				mWorldAABB.mergeAABB((*it)->getWorldAABB(updateFlag));
			}
		}

		upwardSetBoundsDirty();

		NZSetBitFalse(mTransformNBoundUpdateFlag, SOU_BOUND_GEOMETRY);
		NZSetBitFalse(mTransformNBoundUpdateFlag, SOU_BOUND_UPWARD);

		return mWorldAABB;
	}

	void SceneNode::attachChild(SceneNode* child)
	{
		NZ_ASSERT(child);
		NZ_ASSERT(NULL == child->getParentSceneNode());

		SceneNodeList::iterator it = find(mChildren.begin(), mChildren.end(), child);
		if(it != mChildren.end())
		{
			NZWarning("scene node %s already attached to node %s", child->getName().c_str(), mName.c_str());
			return;
		}

		if(preAttachChild(child))
		{
			mChildren.push_back(child);

			child->mParentSceneNode = this;
			child->updateTransformNBounds(0, SOU_WORLDTRANSFORM | SOU_WORLDTRANSFORM_MATRIX | SOU_CHILDREN);

			upwardSetBoundsDirty();

			onAttachChild(child);
		}
	}

	SceneNode* SceneNode::getChild(u32 i)
	{
		if(i < mChildren.size())
		{
			return mChildren[i];
		}

		NZWarning("Invliad index %d for getChild, node %s", i, mName.c_str());
		return NULL;
	}

	SceneNode* SceneNode::getChild(const _string& name)
	{
		for(SceneNodeList::iterator it = mChildren.begin(); it != mChildren.end(); it++)
		{
			if((*it)->getName() == name)
			{
				return (*it);
			}
		}

		NZWarning("Invliad name %d for getChild, node %s", name.c_str(), mName.c_str());
		return NULL;
	}

	void SceneNode::removeChild(SceneNode* child)
	{
		NZ_ASSERT(child);

		SceneNodeList::iterator it = find(mChildren.begin(), mChildren.end(), child);
		if(it != mChildren.end())
		{			
			mChildren.erase(it);
			upwardSetBoundsDirty();
			onRemoveChild(child);
		}
	}

	void SceneNode::removeChild(const _string& name)
	{
		for(SceneNodeList::iterator it = mChildren.begin(); it != mChildren.end(); it++)
		{
			if((*it)->getName() == name)
			{
				mChildren.erase(it);
				upwardSetBoundsDirty();
				onRemoveChild((*it));
				return;
			}
		}
	}

	void SceneNode::removeAllChild()
	{
		for(SceneNodeList::iterator it = mChildren.begin(); it != mChildren.end(); it++)
		{
			onRemoveChild((*it));
		}

		mChildren.clear();

		upwardSetBoundsDirty();
	}

	bool SceneNode::preAttachChild(SceneNode* t)
	{
		// TODO

		return true;
	}

	void SceneNode::onAttachChild(SceneNode* t)
	{
		// TODO delay update bounds or immediately.


	}

	void SceneNode::onRemoveChild(SceneNode* t)
	{
		// TODO
	}

	ContollerJoint* SceneNode::getControllerJoint(bool createIfHasNot /* = false */)
	{
		if(!mControllerJoint && createIfHasNot)
		{
			mControllerJoint = NZ_New ContollerJoint();
		}

		return mControllerJoint;
	}

}//end namespace Nezha